
// import oimo.collision.raycast.*;
// import oimo.common.MathUtil;
// import oimo.common.Transform;
// import oimo.common.Vec3;
// import oimo.m.Vec3;
// import oimo.m.M;

import { ConvexGeometry } from "./ConvexGeometry";
import { Vec3 } from "../../common/Vec3";
import { M } from "../../common/M";
import { Aabb } from "./Aabb";
import { Transform } from "../../common/Transform";
import { RayCastHit } from "./RayCastHit";
import { GeometryType } from "./GeometryType";

/**
 * A box collision geometry.
 */

export class BoxGeometry extends ConvexGeometry {
	_halfExtents: Vec3;
	_halfAxisX: Vec3;
	_halfAxisY: Vec3;
	_halfAxisZ: Vec3;

	/**
	 * Creates a box collision geometry of half-extents `halfExtents`.
	 */
	constructor(halfExtents: Vec3) {
		super(GeometryType._BOX);
		this._halfExtents = halfExtents.clone();
		this._halfAxisX = new Vec3(halfExtents.x, 0, 0);
		this._halfAxisY = new Vec3(0, halfExtents.y, 0);
		this._halfAxisZ = new Vec3(0, 0, halfExtents.z);
		this._updateMass();

		var minHalfExtents: number;
		if (halfExtents.x < halfExtents.y) {
			if (halfExtents.z < halfExtents.x) {
				minHalfExtents = halfExtents.z;
			} else {
				minHalfExtents = halfExtents.x;
			}
		} else {
			if (halfExtents.z < halfExtents.y) {
				minHalfExtents = halfExtents.z;
			} else {
				minHalfExtents = halfExtents.y;
			}
		}
		;

		if (this._gjkMargin > minHalfExtents * 0.2) this._gjkMargin = minHalfExtents * 0.2;
	}

	/**
	 * Returns the half-extents of the box.
	 */
	public getHalfExtents(): Vec3 {
		return this._halfExtents.clone();
	}

	/**
	 * Sets `halfExtents` to the half-extents of the box.
	 */
	public getHalfExtentsTo(halfExtents: Vec3): void {
		halfExtents.copyFrom(this._halfExtents);
	}

	public _updateMass(): void {
		this._volume = 8 * M.vec3_mulHorizontal(this._halfExtents);
		var sq: Vec3 = new Vec3();
		M.vec3_compWiseMul(sq, this._halfExtents, this._halfExtents);
		M.mat3_diagonal(this._inertiaCoeff,
			1 / 3 * (M.vec3_get(sq, 1) + M.vec3_get(sq, 2)),
			1 / 3 * (M.vec3_get(sq, 2) + M.vec3_get(sq, 0)),
			1 / 3 * (M.vec3_get(sq, 0) + M.vec3_get(sq, 1))
		);
	}

	public _computeAabb(aabb: Aabb, tf: Transform): void {
		var tfx: Vec3 = new Vec3();
		var tfy: Vec3 = new Vec3();
		var tfz: Vec3 = new Vec3();
		M.vec3_mulMat3(tfx, this._halfAxisX, tf._rotation);
		M.vec3_mulMat3(tfy, this._halfAxisY, tf._rotation);
		M.vec3_mulMat3(tfz, this._halfAxisZ, tf._rotation);
		tfx.absEq();
		tfy.absEq();
		tfz.absEq();
		var tfs: Vec3 = new Vec3();
		M.vec3_add(tfs, tfx, tfy);
		M.vec3_add(tfs, tfs, tfz);

		M.vec3_sub(aabb._min, tf._position, tfs);
		M.vec3_add(aabb._max, tf._position, tfs);
	}

	public computeLocalSupportingVertex(dir: Vec3, out: Vec3): void {
		var gjkMargins: Vec3 = new Vec3(this._gjkMargin, this._gjkMargin, this._gjkMargin);
		gjkMargins.minEq(this._halfExtents); // avoid making core extents negative
		var coreExtents: Vec3 = new Vec3();
		M.vec3_sub(coreExtents, this._halfExtents, gjkMargins);
		out.x = dir.x > 0 ? M.vec3_get(coreExtents, 0) : -M.vec3_get(coreExtents, 0);
		out.y = dir.y > 0 ? M.vec3_get(coreExtents, 1) : -M.vec3_get(coreExtents, 1);
		out.z = dir.z > 0 ? M.vec3_get(coreExtents, 2) : -M.vec3_get(coreExtents, 2);
	}

	public _rayCastLocal(begin: Vec3, end: Vec3, hit: RayCastHit): boolean {
		var p1x: number = M.vec3_get(begin, 0);
		var p1y: number = M.vec3_get(begin, 1);
		var p1z: number = M.vec3_get(begin, 2);
		var p2x: number = M.vec3_get(end, 0);
		var p2y: number = M.vec3_get(end, 1);
		var p2z: number = M.vec3_get(end, 2);
		var halfW: number = M.vec3_get(this._halfExtents, 0);
		var halfH: number = M.vec3_get(this._halfExtents, 1);
		var halfD: number = M.vec3_get(this._halfExtents, 2);
		var dx: number = p2x - p1x;
		var dy: number = p2y - p1y;
		var dz: number = p2z - p1z;
		var tminx: number = 0;
		var tminy: number = 0;
		var tminz: number = 0;
		var tmaxx: number = 1;
		var tmaxy: number = 1;
		var tmaxz: number = 1;
		if (dx > -1e-6 && dx < 1e-6) {
			if (p1x <= -halfW || p1x >= halfW) {
				return false;
			}
		} else {
			var invDx: number = 1 / dx;
			var t1: number = (-halfW - p1x) * invDx;
			var t2: number = (halfW - p1x) * invDx;
			if (t1 > t2) {
				var tmp: number = t1;
				t1 = t2;
				t2 = tmp;
			}
			if (t1 > 0) tminx = t1;
			if (t2 < 1) tmaxx = t2;
		}

		if (dy > -1e-6 && dy < 1e-6) {
			if (p1y <= -halfH || p1y >= halfH) {
				return false;
			}
		} else {
			var invDy: number = 1 / dy;
			var t1: number = (-halfH - p1y) * invDy;
			var t2: number = (halfH - p1y) * invDy;
			if (t1 > t2) {
				var tmp: number = t1;
				t1 = t2;
				t2 = tmp;
			}
			if (t1 > 0) tminy = t1;
			if (t2 < 1) tmaxy = t2;
		}

		if (dz > -1e-6 && dz < 1e-6) {
			if (p1z <= -halfD || p1z >= halfD) {
				return false;
			}
		} else {
			var invDz: number = 1 / dz;
			var t1: number = (-halfD - p1z) * invDz;
			var t2: number = (halfD - p1z) * invDz;
			if (t1 > t2) {
				var tmp: number = t1;
				t1 = t2;
				t2 = tmp;
			}
			if (t1 > 0) tminz = t1;
			if (t2 < 1) tmaxz = t2;
		}

		if (tminx >= 1 || tminy >= 1 || tminz >= 1 || tmaxx <= 0 || tmaxy <= 0 || tmaxz <= 0) return false;
		var min: number = tminx;
		var max: number = tmaxx;
		var hitDirection: number = 0;
		if (tminy > min) {
			min = tminy;
			hitDirection = 1;
		}
		if (tminz > min) {
			min = tminz;
			hitDirection = 2;
		}
		if (tmaxy < max) {
			max = tmaxy;
		}
		if (tmaxz < max) {
			max = tmaxz;
		}
		if (min > max) return false;
		if (min == 0) return false; // the ray starts from inside
		switch (hitDirection) {
			case 0:
				hit.normal.init(dx > 0 ? -1 : 1, 0, 0);
				break;
			case 1:
				hit.normal.init(0, dy > 0 ? -1 : 1, 0);
				break;
			case 2:
				hit.normal.init(0, 0, dz > 0 ? -1 : 1);
				break;
		}
		hit.position.init(p1x + min * dx, p1y + min * dy, p1z + min * dz);
		hit.fraction = min;
		return true;
	}

}
